// Copyright 2022 Chen Jun
// Licensed under the MIT License.

#ifndef ARMOR_DETECTOR_DETECTOR_HPP_
#define ARMOR_DETECTOR_DETECTOR_HPP_

#include <rclcpp/node.hpp>
// OpenCV
#include <opencv2/core.hpp>
#include <opencv2/core/types.hpp>

// STD
#include "hnurm_detect/armor.hpp"
#include "hnurm_detect/number_classifier.hpp"
#include <memory>
#include <vector>

namespace hnurm
{
class Detector
{
public:
    struct LightParams
    {
        // width / height
        double min_ratio;
        double max_ratio;
        // vertical angle
        double max_angle;
    };

    struct ArmorParams
    {
        double min_light_ratio;
        double min_small_center_distance;
        double max_small_center_distance;
        double min_large_center_distance;
        double max_large_center_distance;
        // horizontal angle
        double max_angle;
    };

    struct ClassifierParams
    {
        std::string              model_path;
        std::string              label_path;
        std::vector<std::string> ignore_classes;
        double                   threshold;
    };

    Detector(
        int                     min_lightness,
        const LightParams      &light_params,
        const ArmorParams      &armor_params,
        const ClassifierParams &classifier_params
    );

    // Debug msgs
    cv::Mat binary_img;

    std::unique_ptr<NumberClassifier> classifier;

    void detect(const cv::Mat &input, int self_color);

    void drawResults(cv::Mat &img);

    cv::Mat getAllNumbersImage();

    [[nodiscard]] cv::Mat preprocessImage(const cv::Mat &input) const;

    std::vector<Light> findLights(const cv::Mat &rbg_img, const cv::Mat &_binary_img);

    std::vector<Armor> matchLights(const std::vector<Light> &lights, int self_color);

    [[nodiscard]] inline std::vector<Armor> &getArmor()
    {
        return armors_;
    }

private:
    bool isLight(const Light &light) const;

    bool isArmor(Armor &armor) const;

    static bool containLight(const Light &light_1, const Light &light_2, const std::vector<Light> &lights);

private:
    std::vector<Light> lights_;
    std::vector<Armor> armors_;

    int         min_lightness {};
    LightParams light_params {};
    ArmorParams armor_params {};

    cv::Point2d calculateMean(const std::vector<cv::Point> &points);

    std::vector<cv::Point2d> centralizeData(const std::vector<cv::Point> &points, const cv::Point2d &mean);

    Eigen::Matrix2d calculateCovarianceMatrix(const std::vector<cv::Point2d> &centralized);

    cv::Point2d performPCA(const std::vector<cv::Point> &points, const cv::Point2d &mean);

    bool
    computeLineIntersection(const cv::Point2d &p1, const cv::Point2d &p2, const cv::Point2d &p3, const cv::Point2d &p4,
                            cv::Point2d &intersection);
};

}  // namespace hnurm

#endif  // ARMOR_DETECTOR_DETECTOR_HPP_
